/*------------------------------------------------------------------------
  File Name          : WingbowHost.h
  Author             : Neil Zhang - Gu
  Version            : V1.0
  Date               : 30/3/2020
  Description        : OpenImu Registers Address

  Analog Devices OpenImu is a complete triple axis gyroscope
  and triple axis accelerometer inertial sensing system

  China Applications Support Team.
  china.support@analog.com
--------------------------------------------------------------------------*/
#include "WingbowHost.h"  


WingbowHost :: WingbowHost()
{
   HostEvent = &Uart5Event;
   HostQueue = &Uart5Queue;
   uartsend = &UART5_Send_Array;                  //串口发送指针
   this->WingbowData.FrameLength = 30;       //返回上位机帧长度
   this->WingbowData.Height[0]=99.0f;
   this->WingbowData.Height[1]=99.0f;
   this->WingbowData.Height[2]=99.0f;
   this->WingbowData.Height[3]=99.0f;    
   WingbowData.ReturnData[0]=HostEvent->frame.head_1;
   WingbowData.ReturnData[1]=HostEvent->frame.head_2;
   WingbowData.ReturnData[28]=HostEvent->frame.tail_1;
   WingbowData.ReturnData[29]=HostEvent->frame.tail_2;
}

WingbowHost *WingbowHost::get(void)
{
    static WingbowHost WingbowHost_;
    return &WingbowHost_;
}



bool WingbowHost :: init(void)
{
    uart5_init(115200); 
    STMFLASH_Write(FLASH_SAVE_ADDR,(u16*)this->WingbowData.Height,sizeof(this->WingbowData.Height));
    GetFlashHeight();                  //开机读取flash中存储的超声高度数据
    ReturnHost(CalibrationUltrasound); //返回上位机信息，串口发送，CRC校验
    ReturnF4(Encoder_Height_Order);    //返回F4 flash中超声高度数据
    return true;

}

u8 WingbowHost :: AnalyWingbowHost()
{
    Queue_Extract(HostQueue); //从接收缓存中提取一帧数据，并更新数组
    if (Uart5Queue.ProceseeReceiveQueue != 0) //如果待处理进程不为0
    {
       switch(HostQueue->Data[2])
       {
           case  CalibrationEncoderZero :  HandleEncoderZero(HostQueue);      break;
           case  CalibrationUltrasound  :  HandleEncoderAngle(HostQueue);     break;
           case  CalibrationEncoderAngle:  HandleUltrasound(HostQueue);       break;
           case  GetImuAngleVel         :  HandleImuAngleVel(HostQueue);      break;
       }           
       ClearQueue(HostQueue);          //每处理一次，清空进程数组
    }
    else
        return 0;
    return 1;
}

/**
  * @brief  上位机标定指定编码器
  * @param  queue：Queue型指针
  * @retval 解析的结果 0：成功，其他：相应的错误码
  */
u8 WingbowHost :: HandleEncoderZero(Queue *queue)
{
   for(u8 i=0;i<4;i++)
   {
     if(queue->Data[2+i])
     {
       if(Encoder_Demarcate(queue->Data[2+i]))     //标定指定编码器
       {
            WingbowData.ReturnData[2+i]=0x00 ;     //标定失败
       }
       else
       {      
            WingbowData.ReturnData[2+i]=0x01 ;     //标定成功
       }
     }
   }
   if(queue->Data[7])
   {
       if(Encoder_Demarcate(queue->Data[7]))     //标定指定编码器
       {
            WingbowData.ReturnData[7]=0x00 ;     //标定失败
       }
       else
       {
            WingbowData.ReturnData[7]=0x01 ;     //标定成功
       }
   }
   
   ReturnHost(CalibrationEncoderZero);            //返回上位机信息，串口发送，CRC校验
   return 1;
}

u8 WingbowHost :: HandleEncoderAngle(Queue *queue)
{
   return 1;
}

u8 WingbowHost :: HandleUltrasound(Queue *queue)
{  
   float *a=NULL;
   
   //循环4次得到队列中的4个超声数据，每个数据4个字节    
   for(u8 i=0;i<4;i++)
   {
     a=(float *)&queue[3+4*i];           //取队列中存储数据的首地址，转换为浮点型指针
     this->WingbowData.Height[i] = *a;   //从指针中取值赋值给超声
   }
   
   //向flash写入上位机传下来的数据
   STMFLASH_Write(FLASH_SAVE_ADDR,(u16*)this->WingbowData.Height,sizeof(this->WingbowData.Height));
   //从flash中读取数据
   GetFlashHeight();
   //返回上位机信息，串口发送，CRC校验
   ReturnHost(CalibrationUltrasound);           
   return 1;
}

u8 WingbowHost :: HandleImuAngleVel(Queue *queue)
{
   float *fAngle=NULL;
   int    angle=0;
   if(queue->Data[3])
   {
        //更新角速度到返回数组
        fAngle = (float *)&WingbowData.ReturnData[3];
        *fAngle = OpenImu::get()->OpenImu_Data.Z_Axis.AngelVel;
        //更新角度到返回数组
        angle = int(OpenImu::get()->OpenImu_Data.Z_Axis.Angle * 1000);
        for (u8 i = 0; i < 4; i++)
        {
            WingbowData.ReturnData[7 + i] = (angle >> (i * 8)) & 0x000000FF; //转换角度
        }
        
        ReturnHost(GetImuAngleVel);            //返回上位机信息，串口发送，CRC校验
   }
   return 1;
}

int  WingbowHost :: CaculateCRC(u8 *queue, u8 framelength)
{
   int sum = 0;
   int *a=0;
   for(u8 i=0;i<framelength-8;i++)
   {
     sum+=queue[2+i];
   }
   a = (int *)&queue[framelength-6] ;
   *a = sum;
   return 1;
}


u8 WingbowHost :: GetFlashHeight()
{
   float *a = NULL;

   this->WingbowData.Height[0] = 0;
   this->WingbowData.Height[1] = 0;
   this->WingbowData.Height[2] = 0;
   this->WingbowData.Height[3] = 0;
   STMFLASH_Read(FLASH_SAVE_ADDR,(u16*)this->WingbowData.Height,sizeof(this->WingbowData.Height));
   //循环4次得到队列中的4个超声数据，每个数据4个字节    
   for(u8 i=0;i<4;i++)
   {
     a=(float *)&WingbowData.ReturnData[3+4*i];  //取队列中存储数据的首地址，转换为浮点型指针
     *a = this->WingbowData.Height[i];           //把写入flash后重新读取的数值写入返回队列
   }
//   ReturnHost(CalibrationUltrasound);            //返回上位机信息，串口发送，CRC校验
   return 1;
}

u8 WingbowHost :: ReturnHost(u8 order)
{
   WingbowData.ReturnData[2] = order ;            //填充命令
   CaculateCRC(WingbowData.ReturnData,WingbowData.FrameLength); //计算CRC
   uartsend(WingbowData.ReturnData, WingbowData.FrameLength)  ;  //返回上位机信息
   memset(&WingbowData.ReturnData[2],0x00,WingbowData.FrameLength-4) ;   //清空除包头包尾外的所有元素
   return 1;
}

u8 WingbowHost :: ReturnF4(u8 order)
{
    u8 sum = 0;
    u8 i = 0;
    float *height=NULL;
    
    Send_Retutn_data[0] = 0x0d;
    Send_Retutn_data[1] = 0x0a;
    Send_Retutn_data[2] = order;
    
    for(u8 i=0;i<4;i++)
    {
        height=(float *)&Send_Retutn_data[3+i*4] ;       //取队列中存储数据的首地址，转换为浮点型指针
        *height = this->WingbowData.Height[i];           //把写入flash后重新读取的数值写入返回队列
    }

    for (i = 0; i < Encoder_Imu_Len - 5; i++)
        sum += Send_Retutn_data[2 + i];
    Send_Retutn_data[Encoder_Imu_Len - 3] = sum;
    Send_Retutn_data[Encoder_Imu_Len - 2] = 0x5a;
    Send_Retutn_data[Encoder_Imu_Len - 1] = 0xa5;

    UART3_Send_Array(Send_Retutn_data, Encoder_Imu_Len);
    Frame_Sign_usart3.now_status = Usart_Initial_number;

    return 0;
}

